/*
upper.c
offer connect upper recive cmd and response api
desigend by lunanting
modified by maoxinfeng: use queue comm between business and agv
------------------------
2020-05-22 16:54:54
create file
------------------------
*/

#include "upper.h"
#include "aos/kernel.h"
#include "string.h"
#include "../bsp/cli_cmd.h"
#include "string.h"
#if defined BATTERY_VERSION
#include "battery/deal_cmd.h"
#endif
#include "../driver/lidar/lidar_flatscan.h"
#include "../driver/lidar/linux_lidar.h"

static UPPER_AGV_class upper={0};

void xp_agv_upper_thread(void *arg);

#ifndef WASHER_MERGE
/*
upper connect init
param:
    port:uart port
    speed:uart speed
return:
    0=success
*/
int xp_agv_upper_init(u8 port,int speed){
    
    upper.init=0;
    upper.uart.port=port;
    upper.uart.config.baud_rate=speed;
    upper.uart.config.data_width=DATA_WIDTH_8BIT;
    upper.uart.config.parity=NO_PARITY;
    upper.uart.config.stop_bits=STOP_BITS_1;
    upper.uart.config.flow_control=FLOW_CONTROL_DISABLED;
    upper.uart.config.mode=MODE_TX_RX;

    if(hal_uart_init(&upper.uart)!=0){
        println("agv upper uart init fail~");
        return -1;
    }

    if(aos_task_new("upper deal with",xp_agv_upper_thread,NULL,3000)){
        return -2;
    }
    println("agv upper connect uart init success~,port=%d,speed=%d",port,speed);
    upper.init=1;
    return 0;
}

/*
agv response upper  recive deal with
*/
void xp_agv_upper_thread(void *arg){
    char buf[120],*p,*data,*foot;
    u32 rec_len,crc;
    u8 count=0;
    println("agv upper deal with thread create success!");
    while(1){
        memset(buf,0,sizeof(buf));
        xp_uart_recive(&upper.uart,(u8 *)buf,100,(int *)&rec_len,5);
        // if(hal_uart_recv_II(&upper.uart,buf,100,&rec_len,20)!=0){
        //     count++;
        //     if(count>200){
        //         count=0;
        //         if(upper.online!=0){
        //             println("upper offline!");
        //         }
        //         upper.online=0;
        //     }

        //     continue;
        // }
        count=0;
        if(upper.online!=1){
            // if(buf[0]==0xff)continue;
            println("===========: %X %X %X\r\n",buf[0],buf[1],buf[2]);
            println("upper online!%s\r\n",buf);
        }
        upper.online=1;
        // println("===========: %X %X %X\r\n",buf[0],buf[1],buf[2]);
        // println("\r\n rec_len:%d %s\r\n",rec_len,buf);
        p=strtok_r(buf,"=;",&foot);
        if(strcmp(p,"crc")==0){
            p=strtok_r(NULL,";",&foot);
            crc=atoi(p);
            if(crc!=xp_crc_check_32((u8 *)foot,strlen(foot)))continue;
        }
        else if(strcmp(p,"agv state")==0){
            println("upper=>%s",buf);
        }
        else{
            continue;
        }
        xp_upper_analysis(foot,strlen(foot));
    }
}

static char send_buf[120];
/*
send data to upper
param:
    data:data buffer
    len:data length
*/
void xp_upper_send(char *data){
    u32 crc=0;

    crc=xp_crc_check_32((u8 *)data,strlen(data));
    sprintf(send_buf,"crc=%d;%s",crc,data);
    //println("send to upper->%s",send_buf);
    hal_uart_send(&upper.uart,send_buf,strlen(send_buf),30);
}

#else
/******************************merge api*****************************/
extern aos_queue_t  g_agv_ctrl_msg_queue;      //AGV控制消息队列
extern aos_queue_t  g_agv_rsp_msg_queue;       //AGV回复消息队列

/*
upper connect init
param:
return:
    0=success
*/
int xp_agv_upper_init(){
    
    upper.init=0;

    if(aos_task_new("agv_upper_deal", xp_agv_upper_thread, NULL, 3000)){
        return -2;
    }
    println("agv upper connect init success~");
    upper.init=1;

    return 0;
}

/*
agv response upper  recive deal with
*/
void xp_agv_upper_thread(void *arg){
    int ret = -1;
    char buf[100], *p, *foot;
    u32 len, crc;

    println("agv upper deal with thread create success!");
    while(1){
        memset(buf, 0, sizeof(buf));
        ret = aos_queue_recv(&g_agv_ctrl_msg_queue, AOS_WAIT_FOREVER, buf, &len);
        if(upper.online != 1){
            println("upper online! len %d buf %s", len, buf);
        }
        upper.online = 1;
        //println("xp_agv_upper_thread: len %d buf %s\r\n", len, buf);
        p = strtok_r(buf, "=;", &foot);
        if(strcmp(p, "crc") == 0){
            p = strtok_r(NULL, ";", &foot);
            crc = atoi(p);
            //消息队列方式下两个字母的CRC校验存在问题，暂时屏蔽
            //if(crc != xp_crc_check_32((u8 *)foot, strlen(foot))) continue;
        }
        else if(strcmp(p, "agv state")==0){
            //println("upper=>%s", buf);
        }
        else{
            continue;
        }
        xp_upper_analysis(foot, strlen(foot));
    }
}

/*
send data to upper
param:
    data:data buffer
    len:data length
*/
void xp_upper_send(char *data){
    u32 crc = 0;
    char send_buf[100] = {0};

    crc = xp_crc_check_32((u8 *)data,strlen(data));
    memset(send_buf, 0, sizeof(send_buf));
    sprintf(send_buf, "crc=%d;%s", crc, data);
    //println("send to upper->%s", send_buf);
    aos_queue_send(&g_agv_rsp_msg_queue, send_buf, sizeof(send_buf));
}
#endif/*WASHER_MERGE*/

UPPER_AGV_class* get_return_upper_hander(void)
{
    return &upper;
}

/*
set agv stoped state
param:
    sta:0=stoped clear,1=stoped
*/
void xp_upper_stoped_set(u8 sta){
    if(sta<2){
        upper.agv.stoped=sta;
    }
}
void xp_upper_move_sate_set(u8 sta)
{
    upper.agv.move_state = sta;
}

void xp_upper_v_lidar_state_set(u8 sta)
{
    upper.agv.v_lidar_error = sta;
}

void xp_upper_h_lidar_state_set(u8 sta)
{
    upper.agv.h_lidar_error = sta;
}

void xp_upper_navi_state_set(u8 sta)
{
    upper.agv.navi_error = sta;
}

void xp_upper_too_near_state_set(u8 sta)
{
    upper.agv.too_near = sta;
}

void xp_upper_set_robot_pose(double angle,double location_x,double location_y)
{    
    upper.agv.angle = (float)angle;
    upper.agv.locate_x = (float)location_x;
    upper.agv.locate_y = (float)location_y;
}

/*
get state message
*/
void xp_upper_get_msg(void){
    int hight = -50;
    int dist = -50;

    // upper.agv.hight=xp_lidar_hight_deal();   //本地单线雷达
    hight = xp_linux_lidar_hight_get();
    if(255 == hight){     //no valid set to -50cm
        hight = -50;
    }

    dist = xp_linux_lidar_dis_get();
    if(255 == dist){
        dist = -50;
    }

    upper.agv.hight = hight * 10;//cm to mm
    upper.agv.dist = dist * 10;//cm to mm
}

/*
get agv state
*/
int xp_upper_get_agv_state(UPPER_AGV_state *p_agv_state){
    if(NULL == p_agv_state){
        println("error: p_agv_state is NULL!");
        return -1;
    }

    xp_upper_get_msg();
    memcpy(p_agv_state, &upper.agv, sizeof(UPPER_AGV_state));

    return 0;
}

/*
upper recive data analysis
param:
    data:analysis data
    len:data length
*/
void xp_upper_analysis(const char *data,u32 len){
    char buf[100];
    int len_s=0;

    if(data==NULL)return;
    if(strcmp(data,UPPER_AGV_req_state)==0){
        xp_upper_get_msg();
        sprintf(buf,"agv state;");
        len_s=strlen(buf);
        memcpy(buf+len_s,&upper.agv,sizeof(UPPER_AGV_state));
        len_s+=sizeof(UPPER_AGV_state);
#ifndef WASHER_MERGE
        hal_uart_send(&upper.uart,buf,len_s,30);
#else
        aos_queue_send(&g_agv_rsp_msg_queue, buf, sizeof(buf));
#endif
    }
    else if(strcmp(data,"agv restart")==0){
        xp_upper_send("ok");
        aos_msleep(100);
        aos_reboot();
    }
    else if(strcmp(data,UPPER_AGV_serch_car)==0){

    }
    else if(xp_upper_user_api(data,len)){}  //user upper api
    else{
        /*
            临时去掉否则主板收不到
        */    
        //hal_uart_send(&upper.uart,UPPER_AGV_undefine,sizeof(UPPER_AGV_undefine),10);
    }
}




/*
define user upper response api
param:
    data: cmd data
    len:cmd length
return:
    1=matched
*/
int __attribute__((weak)) xp_upper_user_api(const char *data,u32 len){
    #if defined BATTERY_VERSION
    xp_upper_solve_control_debug(data);
    #elif defined LINE_VERSION

    #endif
    println("agv upper user api undefine !");
    return 0;
}


/*******************************debug***************************/
int xp_upper_debug(char *type,char *fun,char *param){
    if(strcmp(type,"upper")!=0)return 0;
    if(strcmp(fun,"set_stoped")==0){
        xp_upper_stoped_set(atoi(param));
    }
    else{
        return 0;
    }
    return 1;
}

//end of the file
